#include "MPU9250.h"
MPU9250_SensorData MPU9250_sensorData;
//ASA_X: 174   ASA_Y: 173   ASA_Z: 163  ID:    0x48
u8 AK_ID = 0;
u8 ASA_X = 174;
u8 ASA_Y = 173;
u8 ASA_Z = 163;

float Gyro_Bias_X =-0.010417;
float Gyro_Bias_Y = 0.000737;
float Gyro_Bias_Z =-0.027021;

float Accel_Scale_X = 0.9995;
float Accel_Scale_Y = 0.9997;
float Accel_Scale_Z = 0.9841;
float Accel_Bias_X = -0.0081;
float Accel_Bias_Y =  0.0135;
float Accel_Bias_Z =  0.1843;

float Mag_Bias_X = 0.00260436;
float Mag_Bias_Y = 0.41639228;
float Mag_Bias_Z =-0.50714218;
float Mag_Scale[6] = {0.970014462720998,
-0.00699774522446753,
-0.0174402698381483,
0.988601807599842,
0.056586439235578,
1.04639002054441};
/** How To Use
*	Init_AK8963(&ASA_X, &ASA_Y, &ASA_Z, &AK_ID);
*	delay_ms(100);
*	Init_MPU9250_on_IIC();
*/

u8 MPU9250_Read_Reg(u8 Addr)
{
	u8 RxData;
	CS1_L;
	CS1_L;
	SPI2_ReadWriteByte(Addr|0x80);
	RxData = SPI2_ReadWriteByte(0xff);
	CS1_H;
	delay_us(50);
	return RxData;
}

u8 MPU9250_Write_Reg(u8 Addr,u8 TxData)
{
//	u8 RxData;
	CS1_L;
	CS1_L;
	SPI2_ReadWriteByte(Addr);
	SPI2_ReadWriteByte(TxData);
	CS1_H;
	delay_us(100);
//	CS1_L;
//	CS1_L;
//	SPI2_ReadWriteByte(Addr|0x80);
//	RxData = SPI2_ReadWriteByte(0xff);
//	CS1_H;
//	delay_us(100);
	//判断写入的数据是否正确
//	if(RxData == TxData)
//	{
//		return 0;
//	}else{
//		return 1;
//	}
	return 0;
}

u8 AK8963_Write_RegSPI(u8 Addr,u8 Data)
{
	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,0x00));
	
	while(MPU9250_Write_Reg(I2C_SLV0_ADDR,AK8963_IIC_ADDR>>1));//设置地址且为写
	while(MPU9250_Write_Reg(I2C_SLV0_REG,Addr));
	while(MPU9250_Write_Reg(I2C_SLV0_DO,Data));
	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,0x81));
	delay_ms(20);
//	if(ReadAK8963RegBySPI(Addr)==Data)
//	{
//		return 0;
//	}else
//	{
//		return 1;
//	}
	return 0;
}

u8 AK8963_Read_RegSPI(u8 Addr)
{
	u8 RxData;
	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,0x00));
	
	while(MPU9250_Write_Reg(I2C_SLV0_ADDR,0x80|(AK8963_IIC_ADDR>>1)));//设置地址且为读
	while(MPU9250_Write_Reg(I2C_SLV0_REG,Addr));
	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,0x81));
	delay_ms(20);
	RxData = MPU9250_Read_Reg(EXT_SENS_DATA_00);
	return RxData;
}

void AK8963_Read_Magnetometer(void)
{
	//while(MPU9250_Write_Reg(USER_CTRL,IIC_MST_DISABLE));
	delay_ms(5);
	while(MPU9250_Write_Reg(I2C_SLV0_ADDR,0x80|(AK8963_IIC_ADDR>>1)));//设置地址且为读
	while(MPU9250_Write_Reg(I2C_SLV0_REG,ST1));
	
	while(MPU9250_Write_Reg(I2C_SLV4_CTRL,0x0F));
	while(MPU9250_Write_Reg(I2C_MST_DELAY_CTRL,0x81));
	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,0x88));//读取长度为8
	
	//while(MPU9250_Write_Reg(USER_CTRL,IIC_MST_ENABLE));
	delay_ms(5);
}


void Init_AK8963(u8 *ASA_X, u8 *ASA_Y, u8 *ASA_Z, u8 *whoami)
{
#if USE_SPI_OR_IIC
	Single_Write(MPU9250_IIC_ADDR, INT_PIN_CFG, 0x02);//by-pass enable
	delay_ms(1000);
	Single_Write(AK8963_IIC_ADDR, CNTL2, (0<<7|0<<6|0<<5|0<<4|0<<3|0<<2|0<<1|1<<0) );//restart
	delay_ms(1000);
	if(Single_Read(AK8963_IIC_ADDR, WIA) != 255)
	{*whoami = Single_Read(AK8963_IIC_ADDR, WIA);}//read ID
	delay_ms(100);

	Single_Write(AK8963_IIC_ADDR, CNTL, (0<<7|0<<6|0<<5|1<<4|1<<3|1<<2|1<<1|1<<0) );//Fuse ROM access mode
	delay_ms(100);
	if(Single_Read(AK8963_IIC_ADDR, ASAX) != 255)
	{*ASA_X = Single_Read(AK8963_IIC_ADDR, ASAX);}//read Sensitivity adjustment data for each axis
	delay_ms(10);
	if(Single_Read(AK8963_IIC_ADDR, ASAY) != 255)
	{*ASA_Y = Single_Read(AK8963_IIC_ADDR, ASAY);}
	delay_ms(10);
	if(Single_Read(AK8963_IIC_ADDR, ASAZ) != 255)
	{*ASA_Z = Single_Read(AK8963_IIC_ADDR, ASAZ);}
	delay_ms(10);
	Single_Write(AK8963_IIC_ADDR, CNTL, (0<<7|0<<6|0<<5|1<<4|0<<3|0<<2|0<<1|0<<0) );//power down mode (required!!)
	delay_ms(100);
	Single_Write(AK8963_IIC_ADDR, CNTL, (0<<7|0<<6|0<<5|1<<4|0<<3|1<<2|1<<1|0<<0) );// Continuous measurement mode 2   100Hz refresh rate
	delay_ms(100);
	Single_Write(MPU9250_IIC_ADDR, INT_PIN_CFG, 0x00);//by-pass disable
	delay_ms(100);
#endif
}

void Init_MPU9250()
{
#if USE_SPI_OR_IIC
	Init_AK8963(&ASA_X, &ASA_Y, &ASA_Z, &AK_ID);
	delay_ms(100);
	Single_Write(MPU9250_IIC_ADDR, PWR_MGMT_1, 0x80);//restart
	delay_ms(100);
	Single_Write(MPU9250_IIC_ADDR, PWR_MGMT_1, 0x00);
	delay_ms(10);
	
	Single_Write(MPU9250_IIC_ADDR, SMPLRT_DIV, 0x00);
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, CONFIG, GYRO_92Hz_Bandwidth_1KHz_FS_TEMP_98Hz_Bandwidth);//GYro 92Hz BW tpreture 98Hz BW
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, GYRO_CONFIG, GYRO_FS_SEL_2000dps);
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, ACCEL_CONFIG, ACCEL_FS_SEL_16g);
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, ACCEL_CONFIG_2, ACCEL_41Hz_Bandwidth_1KHz_FS);
	delay_ms(10);
		
	Single_Write(MPU9250_IIC_ADDR, I2C_MST_CTRL, 0x5D);//MST_IIC_400Khz
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_ADDR, (1<<7|AK8963_IIC_ADDR>>1));//Set SLAVE0 ADDR for mag read 
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_REG, ST1);//
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, I2C_SLV0_CTRL, (1<<7|8) );//SLV0 ENABLE& disswap even numbered register ends the group &LEN=8
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, I2C_SLV4_CTRL, 0x09);//1000/(9+1)Hz
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, I2C_MST_DELAY_CTRL, 0x81);
	delay_ms(10);
	Single_Write(MPU9250_IIC_ADDR, USER_CTRL, IIC_MST_ENABLE);//enable master IIC of MPU9250
#else
//	printf("# step 1  \r\n");
//	SPI2_ReadWriteByte(0<<7|PWR_MGMT_1);//写指令+地址
//	SPI2_ReadWriteByte(0x80);//写数据//restart
//	delay_ms(1000);
	MPU9250_Write_Reg(PWR_MGMT_1,0x80);
	delay_ms(10);
	while(MPU9250_Write_Reg(USER_CTRL,0x10));//禁用IIC丛机，只启用SPI丛机
	delay_ms(10);
	while(MPU9250_Write_Reg(PWR_MGMT_1,0x00));
	while(MPU9250_Write_Reg(SMPLRT_DIV,0x00));
	while(MPU9250_Write_Reg(CONFIG,GYRO_92Hz_Bandwidth_1KHz_FS_TEMP_98Hz_Bandwidth));
	while(MPU9250_Write_Reg(GYRO_CONFIG,GYRO_FS_SEL_2000dps));
	while(MPU9250_Write_Reg(ACCEL_CONFIG,ACCEL_FS_SEL_16g));
	while(MPU9250_Write_Reg(ACCEL_CONFIG_2,ACCEL_41Hz_Bandwidth_1KHz_FS));
	
	//while(MPU9250_Write_Reg(INT_PIN_CFG,0x30));
	while(MPU9250_Write_Reg(I2C_MST_CTRL,0x5D));//设置MPU9250 主IIC频率400
	while(MPU9250_Write_Reg(USER_CTRL,IIC_MST_ENABLE));//enable master IIC of MPU9250
	//MPU9250_Write_Reg(I2C_MST_DELAY_CTRL,0x01);
		printf("# step 3  \r\n");
		AK8963_Write_RegSPI(CNTL2,(0<<7|0<<6|0<<5|0<<4|0<<3|0<<2|0<<1|1<<0));//重启
		printf("# step 4  \r\n");
		delay_ms(500);
		delay_ms(500);
		delay_ms(500);
		printf("# CNTL is: 0x%X\r\n",AK8963_Read_RegSPI(CNTL));
		do{
			AK8963_Write_RegSPI(CNTL, (0<<7|0<<6|0<<5|1<<4|1<<3|1<<2|1<<1|1<<0));
		}while(AK8963_Read_RegSPI(CNTL)!=(0<<7|0<<6|0<<5|1<<4|1<<3|1<<2|1<<1|1<<0));
		printf("# CNTL is: 0x%X\r\n",AK8963_Read_RegSPI(CNTL));
		
		printf("# step 5  \r\n");
		
//	if (AK8963_Read_RegSPI(ASAX)!=255&&AK8963_Read_RegSPI(ASAX)!=0)
//	{
		ASA_X = AK8963_Read_RegSPI(ASAX);	
//	}
//	if (AK8963_Read_RegSPI(ASAY)!=255&&AK8963_Read_RegSPI(ASAY)!=0)
//	{
		ASA_Y = AK8963_Read_RegSPI(ASAY);		
//	}
//	if (AK8963_Read_RegSPI(ASAZ)!=255&&AK8963_Read_RegSPI(ASAZ)!=0)
//	{
		ASA_Z = AK8963_Read_RegSPI(ASAZ);
//	}
		printf("# ASA_X is %d\r\n",ASA_X);
		printf("# ASA_Y is %d\r\n",ASA_Y);
		printf("# ASA_Z is %d\r\n",ASA_Z);
		
		AK8963_Write_RegSPI(CNTL, (0<<7|0<<6|0<<5|1<<4|0<<3|0<<2|0<<1|0<<0));
		printf("# step 6  \r\n");
		printf("# CNTL is: 0x%X\r\n",AK8963_Read_RegSPI(CNTL));
		
		do{
			AK8963_Write_RegSPI(CNTL, (0<<7|0<<6|0<<5|1<<4|0<<3|1<<2|1<<1|0<<0));//连续测量模式2 100Hz
		}while(AK8963_Read_RegSPI(CNTL) != (0<<7|0<<6|0<<5|1<<4|0<<3|1<<2|1<<1|0<<0));
		printf("# step 7  \r\n");
		printf("# CNTL is: 0x%X\r\n",AK8963_Read_RegSPI(CNTL));
		
		
		AK8963_Read_Magnetometer();//读取磁力计数据到MPU9250中
//	while(MPU9250_Write_Reg(USER_CTRL,IIC_MST_DISABLE));	
//	while(MPU9250_Write_Reg(I2C_MST_DELAY_CTRL,0x00));
	
//	while(MPU9250_Write_Reg(I2C_SLV0_ADDR,(1<<7)|(AK8963_IIC_ADDR>>1)));
//	while(MPU9250_Write_Reg(I2C_SLV0_REG,ST1));
//	while(MPU9250_Write_Reg(I2C_SLV0_CTRL,(1<<7)|8)){};
//  while(MPU9250_Write_Reg(I2C_SLV4_CTRL,0x01));
		
//	while(MPU9250_Write_Reg(I2C_MST_DELAY_CTRL,0x81));
//	while(MPU9250_Write_Reg(USER_CTRL,IIC_MST_ENABLE));//enable master IIC of MPU9250
#endif
}

void Read_MPU9250(MPU9250_SensorData *sensorData)
	{//读取数据的同时将mpu9250的测量轴系对应转换到  “北东地”的机体轴系上

			u8 BUF[22];
#if USE_SPI_OR_IIC
			Multiple_Read(MPU9250_IIC_ADDR, ACCEL_XOUT_H, 22, BUF);//L3G4200D
#else
			BUF[0] = MPU9250_Read_Reg(ACCEL_XOUT_H);
		  BUF[1] = MPU9250_Read_Reg(ACCEL_XOUT_L);
			BUF[2] = MPU9250_Read_Reg(ACCEL_YOUT_H);
		  BUF[3] = MPU9250_Read_Reg(ACCEL_YOUT_L);
			BUF[4] = MPU9250_Read_Reg(ACCEL_ZOUT_H);
		  BUF[5] = MPU9250_Read_Reg(ACCEL_ZOUT_L);
			BUF[6] = MPU9250_Read_Reg(TEMP_OUT_H);
			BUF[7] = MPU9250_Read_Reg(TEMP_OUT_L);
			BUF[8] = MPU9250_Read_Reg(GYRO_XOUT_H);
		  BUF[9] = MPU9250_Read_Reg(GYRO_XOUT_L);
			BUF[10] = MPU9250_Read_Reg(GYRO_YOUT_H);
		  BUF[11] = MPU9250_Read_Reg(GYRO_YOUT_L);
			BUF[12] = MPU9250_Read_Reg(GYRO_ZOUT_H);
		  BUF[13] = MPU9250_Read_Reg(GYRO_ZOUT_L);
			
			BUF[14] = MPU9250_Read_Reg(EXT_SENS_DATA_00);
			BUF[15] = MPU9250_Read_Reg(EXT_SENS_DATA_01);
			BUF[16] = MPU9250_Read_Reg(EXT_SENS_DATA_02);
			BUF[17] = MPU9250_Read_Reg(EXT_SENS_DATA_03);
			BUF[18] = MPU9250_Read_Reg(EXT_SENS_DATA_04);
			BUF[19] = MPU9250_Read_Reg(EXT_SENS_DATA_05);
			BUF[20] = MPU9250_Read_Reg(EXT_SENS_DATA_06);
			BUF[21] = MPU9250_Read_Reg(EXT_SENS_DATA_07);
#endif
			//accel
			sensorData-> Accel_DataX_raw =  (BUF[2] << 8) | BUF[3];//mpu9250_accel_Y->body_accel_x
			sensorData-> Accel_DataY_raw =	(BUF[0] << 8) | BUF[1];//mpu9250_accel_X->body_accel_y
			sensorData-> Accel_DataZ_raw =	-((BUF[4] << 8) | BUF[5]);//mpu9250_accel_Z->-body_accel_z
			sensorData-> Accel_DataX = sensorData-> Accel_DataX_raw/2048.0;
			sensorData-> Accel_DataY = sensorData-> Accel_DataY_raw/2048.0;
			sensorData-> Accel_DataZ = sensorData-> Accel_DataZ_raw/2048.0;//unit: g
			//tmp
			sensorData-> Tmp_Data_raw =  (BUF[6] << 8) | BUF[7];
			sensorData-> Tmp_Data = (sensorData-> Tmp_Data_raw - 0)/333.87 +21;//TEMP_degC  = ((TEMP_OUT ¡§C RoomTemp_Offset)/Temp_Sensitivity) + 21degC
			//gyro
			sensorData-> Gyro_DataX_raw =  (BUF[10] << 8) | BUF[11];//mpu9250_gyro_Y->body_gyro_x
			sensorData-> Gyro_DataY_raw =  (BUF[8] << 8) | BUF[9];//mpu9250_gyro_X->body_gyro_y
			sensorData-> Gyro_DataZ_raw =  -((BUF[12] << 8) | BUF[13]);//mpu9250_gyro_Z->-body_gyro_z
			sensorData-> Gyro_DataX = sensorData-> Gyro_DataX_raw/16.4*0.0174533;
			sensorData-> Gyro_DataY = sensorData-> Gyro_DataY_raw/16.4*0.0174533;
			sensorData-> Gyro_DataZ = sensorData-> Gyro_DataZ_raw/16.4*0.0174533;//unit: deg*0.0174533->rad!!!
			//*sta1 = BUF[14];
			//mag
			sensorData-> Mag_DataX_raw =  (BUF[16] << 8) | BUF[15];
			sensorData-> Mag_DataY_raw =  (BUF[18] << 8) | BUF[17];
			sensorData-> Mag_DataZ_raw =  ((BUF[20] << 8) | BUF[19]);//0.15uT/LSB
			//The sensitivity adjustment is done by the equation below; 
			sensorData-> Mag_DataX_raw *= ((ASA_X - 128.0)*0.5/128.0 + 1.0);
			sensorData-> Mag_DataY_raw *= ((ASA_Y - 128.0)*0.5/128.0 + 1.0);
			sensorData-> Mag_DataZ_raw *= ((ASA_Z - 128.0)*0.5/128.0 + 1.0);

			sensorData-> Mag_DataX = sensorData-> Mag_DataX_raw * 0.15*0.01;
			sensorData-> Mag_DataY = sensorData-> Mag_DataY_raw * 0.15*0.01;
			sensorData-> Mag_DataZ = sensorData-> Mag_DataZ_raw * 0.15*0.01;//uT*0.01->Gass
			//*sta2 = BUF[21];		
}

void getMPU9250CalibratedData(MPU9250_SensorData *sensorData)
{//数据归一化稍后放在姿态解算里面，因为归一化不涉及传感器本省的特性；校准参数应该与机体轴系对应而不是测量轴系
	float norm;
	float mtx,mty,mtz;
	//Calibrate Gyro
	sensorData->Gyro_DataX_cal = sensorData->Gyro_DataX - Gyro_Bias_X;
	sensorData->Gyro_DataY_cal = sensorData->Gyro_DataY - Gyro_Bias_Y;
	sensorData->Gyro_DataZ_cal = sensorData->Gyro_DataZ - Gyro_Bias_Z;
	
	//Calibrate Accel
	sensorData->Accel_DataX_cal = Accel_Scale_X*(sensorData->Accel_DataX) + Accel_Bias_X;
	sensorData->Accel_DataY_cal = Accel_Scale_Y*(sensorData->Accel_DataY) + Accel_Bias_Y;
	sensorData->Accel_DataZ_cal = Accel_Scale_Z*(sensorData->Accel_DataZ) + Accel_Bias_Z;
	
	norm = sqrt((sensorData->Accel_DataX_cal*sensorData->Accel_DataX_cal + sensorData->Accel_DataY_cal*sensorData->Accel_DataY_cal + sensorData->Accel_DataZ_cal*sensorData->Accel_DataZ_cal));
	sensorData->Accel_DataX_cal = sensorData->Accel_DataX_cal/norm;
	sensorData->Accel_DataY_cal = sensorData->Accel_DataY_cal/norm;
	sensorData->Accel_DataZ_cal = sensorData->Accel_DataZ_cal/norm;
	//Calibrate Mag
	mtx = sensorData->Mag_DataX - Mag_Bias_X;
	mty = sensorData->Mag_DataY - Mag_Bias_Y;
	mtz = sensorData->Mag_DataZ - Mag_Bias_Z;
	sensorData->Mag_DataX_cal = Mag_Scale [0]*mtx + Mag_Scale [1]*mty + Mag_Scale [2]*mtz;
	sensorData->Mag_DataY_cal = Mag_Scale [1]*mtx + Mag_Scale [3]*mty + Mag_Scale [4]*mtz;
	sensorData->Mag_DataZ_cal = Mag_Scale [2]*mtx + Mag_Scale [4]*mty + Mag_Scale [5]*mtz;
	
	norm = sqrt(sensorData->Mag_DataX_cal*sensorData->Mag_DataX_cal + sensorData->Mag_DataY_cal*sensorData->Mag_DataY_cal + sensorData->Mag_DataZ_cal*sensorData->Mag_DataZ_cal);
	sensorData->Mag_DataX_cal=sensorData->Mag_DataX_cal/norm;
	sensorData->Mag_DataY_cal=sensorData->Mag_DataY_cal/norm;
	sensorData->Mag_DataZ_cal=sensorData->Mag_DataZ_cal/norm;
	
	
	//加速度数据软件滤波20Hz
	sensorData->Accel_DataX_cal = ButterworthFilter_one_order_4(sensorData->Accel_DataX_cal);
	sensorData->Accel_DataY_cal = ButterworthFilter_one_order_5(sensorData->Accel_DataY_cal);
	sensorData->Accel_DataZ_cal = ButterworthFilter_one_order_6(sensorData->Accel_DataZ_cal);
	//陀螺仪数据过高通2Hz
	sensorData->Gyro_DataX_cal = ButterworthFilter_one_order_7(sensorData->Gyro_DataX_cal);
	sensorData->Gyro_DataY_cal = ButterworthFilter_one_order_8(sensorData->Gyro_DataY_cal);
	sensorData->Gyro_DataZ_cal = ButterworthFilter_one_order_9(sensorData->Gyro_DataZ_cal);
}

void calMPU9250GyroBiasXYZ(u16 readCont)
{
	u16 i;//表示读取陀螺仪的次数
	
	float tempBiasX = 0;
	float tempBiasY = 0;
	float tempBiasZ = 0;
	
	for(i=1;i<=readCont;i++)
	{
		Read_MPU9250(&MPU9250_sensorData);
		tempBiasX += MPU9250_sensorData.Gyro_DataX;
		tempBiasY += MPU9250_sensorData.Gyro_DataY;
		tempBiasZ += MPU9250_sensorData.Gyro_DataZ;
	}
	Gyro_Bias_X = tempBiasX/readCont;
	Gyro_Bias_Y = tempBiasY/readCont;
	Gyro_Bias_Z = tempBiasZ/readCont;
}
